{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "e8a7b5c5",
   "metadata": {},
   "source": [
    "When using a single move_to_pose command, the planner solves IK for only the start and end poses. It then interpolates joint angles, not Cartesian coordinates.\n",
    "Because the mapping from joint space to Cartesian space is nonlinear, the end-effector path may curve, and the orientation may drift in between.\n",
    "\n",
    "So here we introduce Cartesian Path.\n",
    "\n",
    "A Cartesian path “locks in” the desired position and orientation throughout the trajectory by controlling the end-effector’s pose at every step, rather than only at the start and end."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "58b4508e",
   "metadata": {},
   "outputs": [],
   "source": [
    "import panda_py\n",
    "from panda_py import libfranka\n",
    "\n",
    "panda = panda_py.Panda(\"172.16.0.2\")\n",
    "hand = libfranka.Gripper(\"172.16.0.2\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "4a3a17d6",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[ 9.99986911e-01  2.56996079e-03 -5.66417267e-04  3.07163874e-01]\n",
      " [ 2.56910918e-03 -9.99985949e-01 -1.49915749e-03 -6.35851518e-04]\n",
      " [-5.70262084e-04  1.49768268e-03 -9.99998716e-01  4.86039589e-01]\n",
      " [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]\n"
     ]
    }
   ],
   "source": [
    "panda.move_to_start()\n",
    "\n",
    "print(panda.get_pose())  # Print the current pose of the robot"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "id": "9b9c1151",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[ 0.9652744  -0.04682368 -0.25697257  0.39523613]\n",
      " [-0.06799795 -0.99491745 -0.07413767 -0.18934725]\n",
      " [-0.2521951   0.0890368  -0.96357081  0.051729  ]\n",
      " [ 0.          0.          0.          1.        ]]\n"
     ]
    }
   ],
   "source": [
    "# direct move\n",
    "target_position = [0.4, -0.2, 0.02]  # x, y, z in meters\n",
    "target_orientation = [1,0,0,0]  # Quaternion [w, z, y, x] for gripper down\n",
    "# Get and print the current pose of the robot\n",
    "\n",
    "panda.move_to_pose(target_position, target_orientation)\n",
    "\n",
    "print(panda.get_pose())  # Print the current pose of the robot"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "id": "efbf09ec",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Generated positions for Cartesian path: [[ 0.30749627 -0.00075103  0.48616736]\n",
      " [ 0.31366318 -0.01403429  0.48616736]\n",
      " [ 0.3198301  -0.02731756  0.48616736]\n",
      " [ 0.32599701 -0.04060082  0.48616736]\n",
      " [ 0.33216393 -0.05388409  0.48616736]\n",
      " [ 0.33833084 -0.06716735  0.48616736]\n",
      " [ 0.34449776 -0.08045062  0.48616736]\n",
      " [ 0.35066468 -0.09373388  0.48616736]\n",
      " [ 0.35683159 -0.10701715  0.48616736]\n",
      " [ 0.36299851 -0.12030041  0.48616736]\n",
      " [ 0.36916542 -0.13358368  0.48616736]\n",
      " [ 0.37533234 -0.14686694  0.48616736]\n",
      " [ 0.38149925 -0.16015021  0.48616736]\n",
      " [ 0.38766617 -0.17343347  0.48616736]\n",
      " [ 0.39383308 -0.18671674  0.48616736]\n",
      " [ 0.4        -0.2         0.48616736]\n",
      " [ 0.4        -0.2         0.45286969]\n",
      " [ 0.4        -0.2         0.41957202]\n",
      " [ 0.4        -0.2         0.38627435]\n",
      " [ 0.4        -0.2         0.35297669]\n",
      " [ 0.4        -0.2         0.31967902]\n",
      " [ 0.4        -0.2         0.28638135]\n",
      " [ 0.4        -0.2         0.25308368]\n",
      " [ 0.4        -0.2         0.21978601]\n",
      " [ 0.4        -0.2         0.18648834]\n",
      " [ 0.4        -0.2         0.15319067]\n",
      " [ 0.4        -0.2         0.11989301]\n",
      " [ 0.4        -0.2         0.08659534]\n",
      " [ 0.4        -0.2         0.05329767]\n",
      " [ 0.4        -0.2         0.02      ]]\n",
      "[[ 0.99448953  0.0049359  -0.10462875  0.39770839]\n",
      " [-0.00798861 -0.99239589 -0.1227502  -0.19664843]\n",
      " [-0.10443903  0.12290963 -0.98690689  0.02825282]\n",
      " [ 0.          0.          0.          1.        ]]\n"
     ]
    }
   ],
   "source": [
    "import numpy as np\n",
    "import time\n",
    "\n",
    "panda.move_to_start()\n",
    "# Define start and end positions for the Cartesian path\n",
    "state=panda.get_pose()\n",
    "\n",
    "start_pos = np.array([state[0][3], state[1][3], state[2][3]])  # Extracting the current position from the pose\n",
    "end_pos1 = np.array([0.4, -0.2, start_pos[2]])  # End position at the same height as start\n",
    "end_pos2 = np.array([0.4, -0.2, 0.02])  # Slightly above the table height\n",
    "\n",
    "# Number of steps in the path\n",
    "num_steps = 30\n",
    "\n",
    "# Generate positions along the path\n",
    "positions = np.vstack((\n",
    "    np.linspace(start_pos, end_pos1, num_steps//2, endpoint=False),  # 前半段：只改 x,y\n",
    "    np.linspace(end_pos1, end_pos2, num_steps//2, endpoint=True)     # 后半段：只改 z\n",
    "))\n",
    "print(\"Generated positions for Cartesian path:\", positions)\n",
    "\n",
    "# \"Look down\" orientation quaternion [w, z, x, y]\n",
    "look_down_orientation = np.array([1.0, 0.0, 0.0, 0.0])\n",
    "\n",
    "# Move along the path\n",
    "for pos in positions:\n",
    "    pose = np.concatenate([pos, look_down_orientation])\n",
    "    panda.move_to_pose(pos,look_down_orientation)\n",
    "\n",
    "print(panda.get_pose())  # Print the current pose of the robot"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "id": "a9207515",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "True"
      ]
     },
     "execution_count": 30,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "hand.grasp(0.02, 0.04, 40)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "id": "fb1928dd",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "True"
      ]
     },
     "execution_count": 31,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "panda.move_to_start()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d7bbcece",
   "metadata": {},
   "outputs": [
    {
     "ename": "NameError",
     "evalue": "name 'panda' is not defined",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mNameError\u001b[0m                                 Traceback (most recent call last)",
      "Cell \u001b[0;32mIn[1], line 1\u001b[0m\n\u001b[0;32m----> 1\u001b[0m cur_pose \u001b[38;5;241m=\u001b[39m panda\u001b[38;5;241m.\u001b[39mget_pose()\n\u001b[1;32m      2\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mCurrent Pose:\u001b[39m\u001b[38;5;124m\"\u001b[39m, cur_pose)\n\u001b[1;32m      3\u001b[0m pose\u001b[38;5;241m=\u001b[39mpanda\u001b[38;5;241m.\u001b[39mget_pose()\n",
      "\u001b[0;31mNameError\u001b[0m: name 'panda' is not defined"
     ]
    }
   ],
   "source": [
    "\n",
    "cur_pose = panda.get_pose()\n",
    "print(\"Current Pose:\", cur_pose)\n",
    "pose=panda.get_pose()\n",
    "\n",
    "theta=-np.pi/4  # 45 degrees\n",
    "\n",
    "Rz=np.array([\n",
    "    [np.cos(theta), -np.sin(theta), 0],\n",
    "    [np.sin(theta), np.cos(theta), 0],\n",
    "    [0, 0, 1]\n",
    "    ])\n",
    "\n",
    "pose[:3,:3]=Rz @ pose[:3,:3]\n",
    "\n",
    "panda.move_to_pose(pose, dq_threshold=5e-4)\n",
    "cur_pose = panda.get_pose()\n",
    "print(\"Current Pose:\", cur_pose)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "base",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.11.7"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
